#include "motiontracking.h"

motiontracking::motiontracking(IplImage* img)
{
	image = cvCreateImage( cvGetSize(img), 8, 3 ); 
	grey = cvCreateImage( cvGetSize(img), 8, 1 );
	prev_grey = cvCreateImage( cvGetSize(img), 8, 1 );
	img_sz = cvGetSize( img );
	pyr_sz = cvSize( img->width+8, img->height/3 ); //pyramidal size for lucas kanade algorithm
	pyrA = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
	pyrB = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
	corner_count = 1;
	cornersA = new CvPoint2D32f[ corner_count ];
	cornersB = new CvPoint2D32f[ corner_count ];
	int j = 0;
	
	cornersA[0].x=0;
	cornersA[0].y=0;
	
	
	
	
	

}

int motiontracking::calculatePositionY(IplImage* img,bar bar)
{
	average=0;
	cornersA[0].x = bar.x + bar.width/2;
	cornersA[0].y = bar.y;
	
	/*
	cornersA[0].x = bar.x;
	cornersA[0].y = bar.y - bar.height/2;
	cornersA[1].x = bar.x + bar.width/2;
	cornersA[1].y = bar.y - bar.height/2;
	cornersA[2].x = bar.x + bar.width;
	cornersA[2].y = bar.y - bar.height/2;
	cornersA[3].x = bar.x;
	cornersA[3].y = bar.y;
	cornersA[4].x = bar.x + bar.width/2;
	cornersA[4].y = bar.y;
	cornersA[5].x = bar.x + bar.width;
	cornersA[5].y = bar.y;
	cornersA[6].x = bar.x;
	cornersA[6].y = bar.y + bar.height/2;
	cornersA[7].x = bar.x + bar.width/2;
	cornersA[7].y = bar.y + bar.height/2;
	cornersA[8].x = bar.x + bar.width;
	cornersA[8].y = bar.y + bar.height/2;
	*/
	cvCopy( img, image, 0 ); //copy frame to image
	cvCvtColor( image, grey, CV_BGR2GRAY );

	cvCalcOpticalFlowPyrLK(prev_grey, grey, pyrA, pyrB, cornersA, cornersB, corner_count, win_size, 5, features_found, feature_errors,cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3 ), 0 );
	
	CV_SWAP( prev_grey, grey, swap_temp );
	/*
	for (int i=0; i < 9 ;i++){	

		average+=cornersB[i].y-cornersA[i].y;	
	}
	*/
	return average = cornersB[0].y-cornersA[0].y;
}